/* Prototype of vision algorithm for structured-light range finder utilizing a
 * single horizontal laser line separated from the camera by a fixed distance.
 */
#include <iostream>
#include <iomanip>

#include <time.h>

#include <cv.h>
#include <highgui.h>

#include "conversions.h"
#include "line.h"

using namespace std;
using namespace cv;

/* for my webcam: */
const double vertical_fov = 39.0; /* degrees */
const double horizontal_fov = 54.0; /* degrees */

const double height = 3.1; /*inches*/

/*row1, angle1, row2, angle2*/
Line row2elevation(127, distance2angle(38.5), 115, distance2angle(66.0));

int horizon_row = row2elevation.f_inv(0.0);

Mat lininess_kernel =
    (Mat_<float>(7,1) << -1,0,0,2,0,0,-1) *
    (Mat_<float>(1,7) << 0,0,1,1,1,0,0);

Mat gaussian = getGaussianKernel(7, 4, CV_32F);
Mat gauss_kernel = gaussian * gaussian.t();


void ts_subtract(struct timespec *result,
                 const struct timespec *time1, const struct timespec *time2) {
  *result = *time1;
  result->tv_sec -= time2->tv_sec;
  if (result->tv_nsec < time2->tv_nsec) {
    /* borrow a second */
    result->tv_nsec += 1000000000L;
    result->tv_sec--;
  }
  result->tv_nsec -= time2->tv_nsec;
}

void mouse_callback(int event, int  x, int y, int flags, void* param)
{
  Point2i *click = (Point2i *)param;
  if (event == CV_EVENT_LBUTTONDOWN) {
    *click = Point2i(x, y);
  }
}

template <typename T>
void printMat(const Mat& m)
{
  int i,j;
  for (i = 0; i < m.rows; i++) {
    for (j = 0; j < m.cols; j++) {
      cout << setw(5) << m.at<T>(i,j) << ", ";
    }
    cout << endl;
  }
}

void printPixel(const Mat &img, const Point2i point)
{
  const Vec3b pixel = img.at<Vec3b>(point);
  cout << "(" << point.x << ", " << point.y << ")  "
    << (int)pixel[0] << ", "
    << (int)pixel[1] << ", "
    << (int)pixel[2] << endl;
}

void processImage(Mat &img, const Point2i &click) {
  Mat yuv_img;
  cvtColor(img, yuv_img, CV_BGR2YCrCb);


  Mat yuv_lininess;
  filter2D(yuv_img, yuv_lininess, CV_32F, gauss_kernel);
  filter2D(yuv_lininess, yuv_lininess, CV_32F, lininess_kernel);

  vector<Mat> yuv;
  split(yuv_lininess, yuv);

  Mat lininess;
  addWeighted(yuv[0], 0.2, yuv[1], 0.8, 0.0, lininess);

  //imshow("y_lininess", yuv[0]/255.0);
  //imshow("u_lininess", yuv[1]/255.0);
  imshow("lininess", lininess/255.0);

  Mat distances;
  resize(lininess, distances, Size(lininess.cols/10,lininess.rows/2),
      0,0, INTER_AREA);

  imshow("distances", distances/255.0);

  Mat brightest;
  sortIdx(distances, brightest, CV_SORT_EVERY_COLUMN | CV_SORT_DESCENDING);

  cout << " -------" << endl;
  for (int col = 0; col < distances.cols; col++) {
    double v_angle, distance;
    int row = brightest.at<int>(0, col);
    double h_angle = ((double)col/distances.cols - 0.5) * horizontal_fov;
    float intensity = distances.at<float>(row, col);
    cout << "col: " << setw(3) << col << " h_angle: " << setw(6) << h_angle;
    if (intensity < 32.0) {
      goto next;
    }
    v_angle = row2elevation.f(row);
    if (v_angle < 0.0) {
      goto next;
    }
    distance = angles_to_distance(v_angle, h_angle);
    cout << " --> "
      << setw(3) << row << " (" << setw(5) << v_angle << ") "
      << setw(5) << distance << " inches "
      << "(@" << setw(5) << intensity << ")";
next:
    cout << endl;
  }

  if (click.x != -1) {
    printPixel(yuv_img, click);
  }
}

int main(int argc, char *argv[])
{
  struct timespec time_now, time_last, time_diff;
  int arg = 0;

  if (argc > 1)
    arg = atoi(argv[1]);
  VideoCapture cap(0);
  if (!cap.isOpened()) {
    cout << "Couldn't open camera" << endl;
    return 1;
  }

  namedWindow("Input", CV_WINDOW_AUTOSIZE);
  Point2i click(-1, -1);
  cvSetMouseCallback("Input", mouse_callback, &click);

  cout << setiosflags(ios::fixed) << setprecision(2);

  cout << "lininess_kernel sum: " << sum(lininess_kernel)[0] << endl;
  
  if (arg == 1) return 0;

  while (waitKey(30) != '\x1b') {
    Mat img;
    cap >> img;
    imshow("Input", img);

    processImage(img, click);

    clock_gettime(CLOCK_MONOTONIC_RAW, &time_now);
    ts_subtract(&time_diff, &time_now, &time_last);
    double fps = 1.0L / (time_diff.tv_sec + time_diff.tv_nsec*1.0e-9L);
    cout << fps << " fps" << endl;
    time_last = time_now;

    click.x = -1;
  }
}

// vim:set sw=2 sts=2 ai cin:
